#include "ros/ros.h"
#include "cyberarm_controller/pp_msg.h"
#include "get_state.hpp"
#include "plan.hpp"
#include "move_interface.hpp"

int MoveJ(const std::vector<double>& position, const double& time, const double& sample_rate)
{
    //! pub/sub初始化
    ros::NodeHandle nh;
    ros::Publisher pub = nh.advertise<cyberarm_controller::pp_msg>("pp_mode", 1000); // 位置
    ros::Subscriber state_msg_pub = nh.subscribe("arm_state", 1000, GetState);
    ros::Rate rate(sample_rate);
    int ret;

    //! 获取当前关节角
    std::vector<double> current_pos = GetCurrentPosition();
    ROS_INFO_STREAM("start_state: " << current_pos[0] << ", " << current_pos[1] << ", "<< current_pos[2] << ", "<< current_pos[3] << ".");

    //! 初始化消息
    std::vector<double> pos_diff(MOTOR_NUM);
    cyberarm_controller::pp_msg t_message[MOTOR_NUM];
    for(unsigned int i = 0; i < MOTOR_NUM; i++)
    {
        pos_diff[i] = std::fabs(GetCurrentPosition()[i] - position[i]);
        t_message[i].id = i + 1;
        t_message[i].cmd_mode = 18;
        t_message[i].Vel = 200;
        t_message[i].limit_cur = 27;
        t_message[i].Pos = position[i];
    }

    //! 规划器进行规划
    JointPlan planner[MOTOR_NUM];
    for(unsigned int i = 0; i < MOTOR_NUM; i++)
    {
        planner[i].pos_start = GetCurrentPosition()[i];
        planner[i].pos_end = position[i];
        planner[i].time_lim = time;
        ret = JointPlanFivePoly(sample_rate, planner[i]);
        if(ret < 0)
        {
            ROS_ERROR("规划器规划失败, MoveJ停止运行...");
            return ret;
        }
        else
        {
            ROS_INFO("规划器规划成功, 轨迹共有%ld个数据点.", planner[i].traj.size());
        }
    }

    //! 执行器顺序执行CSP
    int i = 0;
    while(ros::ok() && i < planner[0].traj.size())
    {
        current_pos = GetCurrentPosition();
        for(unsigned int j = 0; j < MOTOR_NUM; j++)
        {
            pos_diff[j] = std::fabs(current_pos[j] - position[j]);
            unsigned int length = planner[j].traj.size();
            if(i < length)
            {
                if(std::fabs(pos_diff[j]) > PLAN_EPS)
                {
                    t_message[j].Pos = planner[j].traj[i];
                }
            }
            else
            {
                t_message[j].Pos = planner[j].traj[length - 1];
            }
            pub.publish(t_message[j]);
        }
        ROS_INFO("send msg: %f, %f, %f, %f", t_message[0].Pos, t_message[1].Pos, t_message[2].Pos, t_message[3].Pos);
        rate.sleep();
        i++;
        ros::spinOnce();
    }

    return 0;
}

int MoveMotor(const std::vector<double>& position)
{
    //! pub/sub初始化
    static ros::NodeHandle nh;
    static ros::Publisher pub = nh.advertise<cyberarm_controller::pp_msg>("pp_mode", 1000); // 位置
    ros::Subscriber state_msg_pub = nh.subscribe("arm_state", 1000, GetState);
    int ret = 0;
    double sample_rate = 200.0;

    //! 获取当前关节角
    std::vector<double> current_pos = GetCurrentPosition();
    ROS_INFO_STREAM("start_state: " << current_pos[0] << ", " << current_pos[1] << ", "<< current_pos[2] << ", "<< current_pos[3] << ".");

    //! 初始化消息
    ros::Rate r(sample_rate);
    cyberarm_controller::pp_msg t_message[MOTOR_NUM];

    std::vector<double> pos_diff(MOTOR_NUM);
    for(unsigned int i = 0; i < MOTOR_NUM; i++)
    {
        pos_diff[i] = std::fabs(GetCurrentPosition()[i] - position[i]);
        t_message[i].id = i + 1;
        t_message[i].cmd_mode = 18;
        t_message[i].Vel = 200;
        t_message[i].limit_cur = 27;
        t_message[i].Pos = position[i];
    }

    double time = 0.0;
    while((ros::ok() && !(pos_diff[0] <= MOVE_READY_EPS && pos_diff[1] <= MOVE_READY_EPS && pos_diff[2] <= MOVE_READY_EPS && pos_diff[3] <= MOVE_READY_EPS)) || time <= MOVE_READY_TIME)
    // while(ros::ok())
    {
        current_pos = GetCurrentPosition();
        ROS_INFO_STREAM("current_state: " << current_pos[0] << ", " << current_pos[1] << ", "<< current_pos[2] << ", "<< current_pos[3] << ".");
        for(unsigned int i = 0; i < MOTOR_NUM; i++)
        {
            pos_diff[i] = std::fabs(current_pos[i] - position[i]);
            pub.publish(t_message[i]);
        }
        ros::spinOnce();
        time += 1.0 / sample_rate;
        r.sleep();
    }
        

    return ret;

}